/*---------------------------------------------------------------------
 * File name: icm42688p.h
 *
 * Copyright (c) <2020-2022>, <ChenLong>
 *
 * All rights reserved.
 *
 * Author: ChenLong
 * Email: worldlong@foxmail.com
 *--------------------------------------------------------------------*/
#pragma once
/*
*/
#include "gpio.h"
#include "spi.h"
/*
*/
#define ICM42688P_WHO_AM_I_DATA      0x47
//USER BANK 0 REGISTER MAP
#define ICM42688P_DEVICE_CONFIG      0x11
#define ICM42688P_DRIVE_CONFIG       0x13
#define ICM42688P_INT_CONFIG         0x14
#define ICM42688P_FIFO_CONFIG        0x16
#define ICM42688P_TEMP_DATA1         0x1D
#define ICM42688P_TEMP_DATA0         0x1E
#define ICM42688P_ACCEL_DATA_X1      0x1F
#define ICM42688P_ACCEL_DATA_X0      0x20
#define ICM42688P_ACCEL_DATA_Y1      0x21
#define ICM42688P_ACCEL_DATA_Y0      0x22
#define ICM42688P_ACCEL_DATA_Z1      0x23
#define ICM42688P_ACCEL_DATA_Z0      0x24
#define ICM42688P_GYRO_DATA_X1       0x25
#define ICM42688P_GYRO_DATA_X0       0x26
#define ICM42688P_GYRO_DATA_Y1       0x27
#define ICM42688P_GYRO_DATA_Y0       0x28
#define ICM42688P_GYRO_DATA_Z1       0x29
#define ICM42688P_GYRO_DATA_Z0       0x2A
#define ICM42688P_TMST_FSYNCH        0x2B
#define ICM42688P_TMST_FSYNCL        0x2C
#define ICM42688P_INT_STATUS         0x2D
#define ICM42688P_FIFO_COUNTH        0x2E
#define ICM42688P_FIFO_COUNTL        0x2F
#define ICM42688P_FIFO_DATA          0x30
#define ICM42688P_APEX_DATA0         0x31
#define ICM42688P_APEX_DATA1         0x32
#define ICM42688P_APEX_DATA2         0x33
#define ICM42688P_APEX_DATA3         0x34
#define ICM42688P_APEX_DATA4         0x35
#define ICM42688P_APEX_DATA5         0x36
#define ICM42688P_INT_STATUS2        0x37
#define ICM42688P_INT_STATUS3        0x38
#define ICM42688P_SIGNAL_PATH_RESET  0x4B
#define ICM42688P_INTF_CONFIG0       0x4C
#define ICM42688P_INTF_CONFIG1       0x4D
#define ICM42688P_PWR_MGMT0          0x4E
#define ICM42688P_GYRO_CONFIG0       0x4F
#define ICM42688P_ACCEL_CONFIG0      0x50
#define ICM42688P_GYRO_CONFIG1       0x51
#define ICM42688P_GYRO_ACCEL_CONFIG0 0x52
#define ICM42688P_ACCEL_CONFIG1      0x53
#define ICM42688P_TMST_CONFIG        0x54
#define ICM42688P_APEX_CONFIG0       0x56
#define ICM42688P_SMD_CONFIG         0x57
#define ICM42688P_FIFO_CONFIG1       0x5F
#define ICM42688P_FIFO_CONFIG2       0x60
#define ICM42688P_FIFO_CONFIG3       0x61
#define ICM42688P_FSYNC_CONFIG       0x62
#define ICM42688P_INT_CONFIG0        0x63
#define ICM42688P_INT_CONFIG1        0x64
#define ICM42688P_INT_SOURCE0        0x65
#define ICM42688P_INT_SOURCE1        0x66
#define ICM42688P_INT_SOURCE3        0x68
#define ICM42688P_INT_SOURCE4        0x69
#define ICM42688P_FIFO_LOST_PKT0     0x6C
#define ICM42688P_FIFO_LOST_PKT1     0x6D
#define ICM42688P_SELF_TEST_CONFIG   0x70
#define ICM42688P_WHO_AM_I           0x75
#define ICM42688P_REG_BANK_SEL       0x76
//USER BANK 1 REGISTER MAP
#define ICM42688P_SENSOR_CONFIG0        0x03
#define ICM42688P_GYRO_CONFIG_STATIC2   0x0B
#define ICM42688P_GYRO_CONFIG_STATIC3   0x0C
#define ICM42688P_GYRO_CONFIG_STATIC4   0x0D
#define ICM42688P_GYRO_CONFIG_STATIC5   0x0E
#define ICM42688P_GYRO_CONFIG_STATIC6   0x0F
#define ICM42688P_GYRO_CONFIG_STATIC7   0x10
#define ICM42688P_GYRO_CONFIG_STATIC8   0x11
#define ICM42688P_GYRO_CONFIG_STATIC9   0x12
#define ICM42688P_GYRO_CONFIG_STATIC10  0x13
#define ICM42688P_XG_ST_DATA            0x5F
#define ICM42688P_YG_ST_DATA            0x60
#define ICM42688P_ZG_ST_DATA            0x61
#define ICM42688P_TMSTVAL0              0x62
#define ICM42688P_TMSTVAL1              0x63
#define ICM42688P_TMSTVAL2              0x64
#define ICM42688P_INTF_CONFIG4          0x7A
#define ICM42688P_INTF_CONFIG5          0x7B
#define ICM42688P_INTF_CONFIG6          0x7C
//USER BANK 2 REGISTER MAP
#define ICM42688P_ACCEL_CONFIG_STATIC2  0x03
#define ICM42688P_ACCEL_CONFIG_STATIC3  0x04
#define ICM42688P_ACCEL_CONFIG_STATIC4  0x05
#define ICM42688P_XA_ST_DATA            0x3B
#define ICM42688P_YA_ST_DATA            0x3C
#define ICM42688P_ZA_ST_DATA            0x3D
//USER BANK 4 REGISTER MAP
#define ICM42688P_APEX_CONFIG1     0x40
#define ICM42688P_APEX_CONFIG2     0x41
#define ICM42688P_APEX_CONFIG3     0x42
#define ICM42688P_APEX_CONFIG4     0x43
#define ICM42688P_APEX_CONFIG5     0x44
#define ICM42688P_APEX_CONFIG6     0x45
#define ICM42688P_APEX_CONFIG7     0x46
#define ICM42688P_APEX_CONFIG8     0x47
#define ICM42688P_APEX_CONFIG9     0x48
#define ICM42688P_ACCEL_WOM_X_THR  0x4A
#define ICM42688P_ACCEL_WOM_Y_THR  0x4B
#define ICM42688P_ACCEL_WOM_Z_THR  0x4C
#define ICM42688P_INT_SOURCE6      0x4D
#define ICM42688P_INT_SOURCE7      0x4E
#define ICM42688P_INT_SOURCE8      0x4F
#define ICM42688P_INT_SOURCE9      0x50
#define ICM42688P_INT_SOURCE10     0x51
#define ICM42688P_OFFSET_USER0     0x77
#define ICM42688P_OFFSET_USER1     0x78
#define ICM42688P_OFFSET_USER2     0x79
#define ICM42688P_OFFSET_USER3     0x7A
#define ICM42688P_OFFSET_USER4     0x7B
#define ICM42688P_OFFSET_USER5     0x7C
#define ICM42688P_OFFSET_USER6     0x7D
#define ICM42688P_OFFSET_USER7     0x7E
#define ICM42688P_OFFSET_USER8     0x7F
/*
*/
class ICM42688P 
{
public:
  ICM42688P(hal::SPI *spi, hal::GPIO *cs, int ncs) :
    accel_scale(1.0f),
    gyro_scale(1.0f),
    _spi(spi),
    _cs(cs),
    _ncs(ncs)
    {}
    
  float accel_scale;
  float gyro_scale;
  
  /* After reset, must wait 1ms for soft reset to be effective, 
     before attempting any other register access. */
  void reset();
  /* Check Who am I */
  bool check();
  
  /* After config, must wait 200us for next operation */
  bool config(int accel_fs, int gyro_fs, int odr);
  bool config(uint8_t *cfg[2], int size);
  void read_raw(int16_t *temp, int16_t *accel, int16_t *gyro);
  void read_data(uint8_t *data) {
    read_bytes(data, ICM42688P_TEMP_DATA1, 14);
  }
  void flush() {
    read_bytes(0, ICM42688P_TEMP_DATA1, 14);
  }
  
private:
  hal::SPI *_spi;
  hal::GPIO *_cs;
  int _ncs;
  
  void write_reg(uint8_t addr, uint8_t data);
  bool write_reg_with_check(uint8_t addr, uint8_t data);
  uint8_t read_reg(uint8_t addr);
  int read_bytes(uint8_t *data, uint8_t addr, int number);
};
